/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.mechanatrix.loco;

import com.jme3.asset.AssetManager;
import com.jme3.bullet.PhysicsSpace;
import com.jme3.bullet.collision.PhysicsCollisionObject;
import com.jme3.bullet.collision.shapes.CompoundCollisionShape;
import com.jme3.bullet.collision.shapes.CylinderCollisionShape;
import com.jme3.bullet.control.VehicleControl;
import com.jme3.material.Material;
import com.jme3.math.ColorRGBA;
import com.jme3.math.FastMath;
import com.jme3.math.Vector3f;
import com.jme3.renderer.queue.RenderQueue;
import com.jme3.scene.Geometry;
import com.jme3.scene.Mesh;
import com.jme3.scene.Node;
import com.jme3.scene.Spatial;
import com.jme3.scene.VertexBuffer;
import com.jme3.scene.shape.Cylinder;
import com.jme3.util.BufferUtils;

/**
 *
 * @author Dustin
 */
public class Locomotive extends Node {

	//Vital Stats
	private float scale = 1.0f;
	private VehicleControl vehicle;
	private Node rootNode;
	private AssetManager assetManager;
	private PhysicsSpace space;
	//Scene for the locomotive.
	Spatial model;
	//Handle for the locomotive.
	Spatial objectNode;
	//Main Body
	Spatial mainBody;
	//Bogies
	final Bogie northBogie = new Bogie(true, false); //steering
	final Bogie centerBogie = new Bogie();
	final Bogie southBogie = new Bogie(false, true); //south brake
	//Ends
	final End northEnd = new End();
	final End southEnd = new End(true); //rear

	public static Locomotive createLocomotive(Node rootNode, AssetManager assetManager, PhysicsSpace space) {

		//TODO Make it so that we only need the asset manager here. Do we need physics here? Maybe...

		Locomotive loco = new Locomotive();
		loco.rootNode = rootNode;
		loco.assetManager = assetManager;
		loco.space = space;
		loco.loadModel();

		//Material material = new Material(assetManager, "Common/MatDefs/Misc/Unshaded.j3md");
		//material.getAdditionalRenderState().setWireframe(true);
		//material.setColor("Color", ColorRGBA.Red);
		Material material = new Material(assetManager, "Common/MatDefs/Misc/ShowNormals.j3md");

		//create a compound shape and attach the BoxCollisionShape for the car body at 0,1,0
		//this shifts the effective center of mass of the BoxCollisionShape to 0,-1,0
		CompoundCollisionShape compoundShape = new CompoundCollisionShape();
		//BoxCollisionShape hingeBox = new BoxCollisionShape(new Vector3f(2.5f * scale, 1.25f * scale, 11.75f * scale));
		//compoundShape.addChildShape(hingeBox, new Vector3f(0f, 2.5f * scale, 0f));
		CylinderCollisionShape tube = new CylinderCollisionShape(new Vector3f(2.5f * loco.scale, 16f, 11.75f * loco.scale));
		//CollisionShape box = CollisionShapeFactory.createDynamicMeshShape(l.mainBody);
		compoundShape.addChildShape(tube, new Vector3f(0f, 3f * loco.scale, 0f));


		loco.vehicle = new VehicleControl(compoundShape, 400);
		loco.vehicle.setCollisionGroup(PhysicsCollisionObject.COLLISION_GROUP_02);
		loco.addControl(loco.vehicle);



		//setting suspension values for wheels, this can be a bit tricky
		//see also https://docs.google.com/Doc?docid=0AXVUZ5xw6XpKZGNuZG56a3FfMzU0Z2NyZnF4Zmo&hl=en
		float stiffness = 15.0f;//200=f1 car
		float compValue = .3f; //(should be lower than damp)
		float dampValue = .4f;
		loco.vehicle.setSuspensionCompression(compValue * 2.0f * FastMath.sqrt(stiffness));
		loco.vehicle.setSuspensionDamping(dampValue * 2.0f * FastMath.sqrt(stiffness));
		loco.vehicle.setSuspensionStiffness(stiffness);
		loco.vehicle.setMaxSuspensionForce(10000.0f);

		//Create four wheels and add them at their locations
		Vector3f wheelDirection = new Vector3f(0, -1, 0); // was 0, -1, 0
		Vector3f axle = new Vector3f(-1, 0, 0); // was -1, 0, 0
		float wheelRadius = 0.67f * loco.scale;
		float restLength = 0.75f * loco.scale;
		float wheelHeight = 0.72f * 2f * loco.scale;
		//float wheelHeight = wheelRadius * 2f;
		float bogieWidth = 5.4f * loco.scale;
		float wheelSpacing = 2.6f * loco.scale;
		float bogieSpacing = 7.6f * loco.scale;


		//Cylinder wheelMesh = new Cylinder(16, 16, wheelRadius, wheelRadius * 0.6f, true);
		

		loco.northBogie.westWheelBox.northWheel.removeFromParent();
		loco.northBogie.westWheelBox.southWheel.removeFromParent();
		loco.northBogie.eastWheelBox.northWheel.removeFromParent();
		loco.northBogie.eastWheelBox.southWheel.removeFromParent();
		
		loco.northBogie.westWheelBox.breakClamp.removeFromParent();
		loco.northBogie.eastWheelBox.breakClamp.removeFromParent();
		
		//loco.northBogie.westWheelBox.lowerArm.removeFromParent();
		//loco.northBogie.westWheelBox.outerArm.removeFromParent();
		//loco.northBogie.westWheelBox.upperArm.removeFromParent();
		//loco.northBogie.westWheelBox.wheelBox.removeFromParent();
		
		//loco.northBogie.eastWheelBox.lowerArm.removeFromParent();
		//loco.northBogie.eastWheelBox.outerArm.removeFromParent();
		//loco.northBogie.eastWheelBox.upperArm.removeFromParent();
		//loco.northBogie.eastWheelBox.wheelBox.removeFromParent();
		
		
		loco.centerBogie.westWheelBox.northWheel.removeFromParent();
		loco.centerBogie.westWheelBox.southWheel.removeFromParent();
		loco.centerBogie.eastWheelBox.northWheel.removeFromParent();
		loco.centerBogie.eastWheelBox.southWheel.removeFromParent();
		
		//loco.centerBogie.westWheelBox.breakClamp.removeFromParent();
		//loco.centerBogie.eastWheelBox.breakClamp.removeFromParent();
		
		//loco.centerBogie.westWheelBox.lowerArm.removeFromParent();
		//loco.centerBogie.westWheelBox.outerArm.removeFromParent();
		//loco.centerBogie.westWheelBox.upperArm.removeFromParent();
		//loco.centerBogie.westWheelBox.wheelBox.removeFromParent();
		
		//loco.centerBogie.eastWheelBox.lowerArm.removeFromParent();
		//loco.centerBogie.eastWheelBox.outerArm.removeFromParent();
		//loco.centerBogie.eastWheelBox.upperArm.removeFromParent();
		//loco.centerBogie.eastWheelBox.wheelBox.removeFromParent();
		
		
		loco.southBogie.westWheelBox.northWheel.removeFromParent();
		loco.southBogie.westWheelBox.southWheel.removeFromParent();
		loco.southBogie.eastWheelBox.northWheel.removeFromParent();
		loco.southBogie.eastWheelBox.southWheel.removeFromParent();
		
		//loco.southBogie.westWheelBox.breakClamp.removeFromParent();
		//loco.southBogie.eastWheelBox.breakClamp.removeFromParent();
		
		//loco.southBogie.westWheelBox.lowerArm.removeFromParent();
		//loco.southBogie.westWheelBox.outerArm.removeFromParent();
		//loco.southBogie.westWheelBox.upperArm.removeFromParent();
		//loco.southBogie.westWheelBox.wheelBox.removeFromParent();
		
		//loco.southBogie.eastWheelBox.lowerArm.removeFromParent();
		//loco.southBogie.eastWheelBox.outerArm.removeFromParent();
		//loco.southBogie.eastWheelBox.upperArm.removeFromParent();
		//loco.southBogie.eastWheelBox.wheelBox.removeFromParent();

		
		//float wheelScale = 0.067f; // wheelRadius / 20
		//float wheelScale = wheelRadius / 20f;
		float wheelScale = wheelRadius * 2f; //TODO Adjust!
		
		Spatial westWheel = assetManager.loadModel("Models/wheels/LeftTire.obj");
		westWheel.scale(wheelScale);
		//westWheel.setMaterial(material);
		
		Spatial eastWheel = assetManager.loadModel("Models/wheels/RightTire.obj");
		eastWheel.scale(wheelScale);
		//eastWheel.setMaterial(material);
		/*
		Node node1 = new Node("front left wheel of front axle node");
		Geometry wheels1 = new Geometry("front left wheel of front axle", wheelMesh);
		node1.attachChild(wheels1);
		wheels1.rotate(0, FastMath.HALF_PI, 0);
		wheels1.setMaterial(material);
		*/
		Spatial node1 = westWheel.clone(true);
		loco.vehicle.addWheel(node1, new Vector3f(-bogieWidth / 2f, wheelHeight, (wheelSpacing / 2f) + bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, true);
		/*
		Node node1 = new Node("front left wheel of front axle node");
		Spatial wheel1 = loco.northBogie.westWheelBox.northWheel;
		node1.attachChild(wheel1);
		wheel1.setLocalTranslation(-bogieWidth / 2f, wheelHeight, -(wheelSpacing / 2f) - bogieSpacing);
		//wheel1.move(-bogieWidth / 2f, 0, -(wheelSpacing / 2f) - bogieSpacing);
		loco.vehicle.addWheel(node1, new Vector3f(-bogieWidth / 2f, wheelHeight, (wheelSpacing / 2f) + bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, true);
		*/
		
		/*
		Node node2 = new Node("front right wheel of front axle node");
		Geometry wheels2 = new Geometry("front right wheel of front axle", wheelMesh);
		node2.attachChild(wheels2);
		wheels2.rotate(0, FastMath.HALF_PI, 0);
		wheels2.setMaterial(material);
		*/
		Spatial node2 = eastWheel.clone(true);
		loco.vehicle.addWheel(node2, new Vector3f(bogieWidth / 2f, wheelHeight, (wheelSpacing / 2f) + bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, true);

		/*
		Node node3 = new Node("rear left wheel of front axle node");
		Geometry wheels3 = new Geometry("rear left wheel of front axle", wheelMesh);
		node3.attachChild(wheels3);
		wheels3.rotate(0, FastMath.HALF_PI, 0);
		wheels3.setMaterial(material);
		*/
		Spatial node3 = westWheel.clone(true);
		loco.vehicle.addWheel(node3, new Vector3f(-bogieWidth / 2f, wheelHeight, (wheelSpacing / -2f) + bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, true);

		/*
		Node node4 = new Node("rear right wheel of front axle node");
		Geometry wheels4 = new Geometry("rear right wheel of front axle", wheelMesh);
		node4.attachChild(wheels4);
		wheels4.rotate(0, FastMath.HALF_PI, 0);
		wheels4.setMaterial(material);
		*/
		Spatial node4 = eastWheel.clone(true);
		loco.vehicle.addWheel(node4, new Vector3f(bogieWidth / 2f, wheelHeight, (wheelSpacing / -2f) + bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, true);

		/*
		Node node5 = new Node("front left wheel of center axle node");
		Geometry wheels5 = new Geometry("front left wheel of center axle", wheelMesh);
		node5.attachChild(wheels5);
		wheels5.rotate(0, FastMath.HALF_PI, 0);
		wheels5.setMaterial(material);
		*/
		Spatial node5 = westWheel.clone(true);
		loco.vehicle.addWheel(node5, new Vector3f(-bogieWidth / 2f, wheelHeight, wheelSpacing / 2f),
				wheelDirection, axle, restLength, wheelRadius, false);
		/*
		Node node6 = new Node("front right wheel of center axle node");
		Geometry wheels6 = new Geometry("front right wheel of center axle", wheelMesh);
		node6.attachChild(wheels6);
		wheels6.rotate(0, FastMath.HALF_PI, 0);
		wheels6.setMaterial(material);
		*/
		Spatial node6 = eastWheel.clone(true);
		loco.vehicle.addWheel(node6, new Vector3f(bogieWidth / 2f, wheelHeight, wheelSpacing / 2f),
				wheelDirection, axle, restLength, wheelRadius, false);
		/*
		Node node7 = new Node("rear left wheel of center axle node");
		Geometry wheels7 = new Geometry("rear left wheel of center axle", wheelMesh);
		node7.attachChild(wheels7);
		wheels7.rotate(0, FastMath.HALF_PI, 0);
		wheels7.setMaterial(material);
		*/
		Spatial node7 = westWheel.clone(true);
		loco.vehicle.addWheel(node7, new Vector3f(-bogieWidth / 2f, wheelHeight, wheelSpacing / -2f),
				wheelDirection, axle, restLength, wheelRadius, false);

		/*
		Node node8 = new Node("rear right wheel of center axle node");
		Geometry wheels8 = new Geometry("rear right wheel of center axle", wheelMesh);
		node8.attachChild(wheels8);
		wheels8.rotate(0, FastMath.HALF_PI, 0);
		wheels8.setMaterial(material);
		*/
		Spatial node8 = eastWheel.clone(true);
		loco.vehicle.addWheel(node8, new Vector3f(bogieWidth / 2f, wheelHeight, wheelSpacing / -2f),
				wheelDirection, axle, restLength, wheelRadius, false);

		/*
		Node node9 = new Node("front left wheel of rear axle node");
		Geometry wheels9 = new Geometry("front left wheel of rear axle", wheelMesh);
		node9.attachChild(wheels9);
		wheels9.rotate(0, FastMath.HALF_PI, 0);
		wheels9.setMaterial(material);
		*/
		Spatial node9 = westWheel.clone(true);
		loco.vehicle.addWheel(node9, new Vector3f(-bogieWidth / 2f, wheelHeight, (wheelSpacing / 2f) - bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, false);

		/*
		Node node10 = new Node("front right wheel of rear axle node");
		Geometry wheels10 = new Geometry("front right wheel of rear axle", wheelMesh);
		node10.attachChild(wheels10);
		wheels10.rotate(0, FastMath.HALF_PI, 0);
		wheels10.setMaterial(material);
		*/
		Spatial node10 = eastWheel.clone(true);
		loco.vehicle.addWheel(node10, new Vector3f(bogieWidth / 2f, wheelHeight, (wheelSpacing / 2f) - bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, false);
		/*
		Node node11 = new Node("rear left wheel of rear axle node");
		Geometry wheels11 = new Geometry("rear left wheel of rear axle", wheelMesh);
		node11.attachChild(wheels11);
		wheels11.rotate(0, FastMath.HALF_PI, 0);
		wheels11.setMaterial(material);
		*/
		Spatial node11 = westWheel.clone(true);
		loco.vehicle.addWheel(node11, new Vector3f(-bogieWidth / 2f, wheelHeight, (wheelSpacing / -2f) - bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, false);

		/*
		Node node12 = new Node("rear right wheel of rear axle node");
		Geometry wheels12 = new Geometry("rear right wheel of rear axle", wheelMesh);
		node12.attachChild(wheels12);
		wheels12.rotate(0, FastMath.HALF_PI, 0);
		wheels12.setMaterial(material);
		*/
		Spatial node12 = eastWheel.clone(true);
		loco.vehicle.addWheel(node12, new Vector3f(bogieWidth / 2f, wheelHeight, (wheelSpacing / -2f) - bogieSpacing),
				wheelDirection, axle, restLength, wheelRadius, false);

		loco.attachChild(node1);
		loco.attachChild(node2);
		loco.attachChild(node3);
		loco.attachChild(node4);
		loco.attachChild(node5);
		loco.attachChild(node6);
		loco.attachChild(node7);
		loco.attachChild(node8);
		loco.attachChild(node9);
		loco.attachChild(node10);
		loco.attachChild(node11);
		loco.attachChild(node12);
		rootNode.attachChild(loco);

		space.add(loco.vehicle);

		return loco;
	}

	public void plotLine(Vector3f[] lineVerticies, ColorRGBA lineColor) {
		Mesh m = new Mesh();
		m.setMode(Mesh.Mode.Lines);

		m.setBuffer(VertexBuffer.Type.Position, 3, BufferUtils.createFloatBuffer(lineVerticies));

		short[] indexes = new short[2 * lineVerticies.length]; //Indexes are in pairs, from a vertex and to a vertex

		for (short i = 0; i < lineVerticies.length - 1; i++) {
			indexes[2 * i] = i;
			indexes[2 * i + 1] = (short) (i + 1);
		}

		m.setBuffer(VertexBuffer.Type.Index, 2, indexes);

		m.updateBound();
		m.updateCounts();

		Geometry geo = new Geometry("line", m);
		Material mat = new Material(assetManager, "Common/MatDefs/Misc/Unshaded.j3md");
		mat.setColor("Color", lineColor);
		geo.setMaterial(mat);

		attachChild(geo);
	}

	public VehicleControl getVehicleControl() {
		return vehicle;
	}

	public void setPhysicsSpace(PhysicsSpace space) {
		this.space = space;
		space.add(vehicle);
	}

	public PhysicsSpace getPhysicsSpace() {
		return space;
	}

	public Node getRootNode() {
		return rootNode;
	}

	private void loadModel() {

		//box aboave bogie
		//BoxCollisionShape armBox = new BoxCollisionShape(new Vector3f(0.72f * loco.scale, 0.2f * loco.scale, wheelSpacing * 0.29f));
		//compoundShape.addChildShape(armBox, new Vector3f(bogieWidth / 2f - 0.027f * loco.scale, 1.42f * loco.scale, /*bogieSpacing*/ 0f));
		//length, height, width
		//out, up, forward

		//Center box
		//BoxCollisionShape armSpace = new BoxCollisionShape(new Vector3f(0.444f * l.scale, 0.14f * l.scale, 0.444f * l.scale));
		//compoundShape.addChildShape(armSpace, new Vector3f(bogieWidth / 2f - 0.027f * l.scale, 1.42f * l.scale, /*bogieSpacing*/0f));
		//space inside box
		//BoxCollisionShape armSpace = new BoxCollisionShape(new Vector3f(0.594f * loco.scale, 0.14f * loco.scale, 0.444f * loco.scale));
		//compoundShape.addChildShape(armSpace, new Vector3f((bogieWidth / 2f) + (0.123f * loco.scale), 1.38f * loco.scale, /*bogieSpacing*/ 0f));

		/*
		 Vector3f[] topHingeLine = new Vector3f[2];
		 topHingeLine[0] = new Vector3f(3.55f * loco.scale, 1.4f * loco.scale, 12 * loco.scale); //out, up, forward
		 topHingeLine[1] = new Vector3f(3.55f * loco.scale, 1.4f * loco.scale, -12 * loco.scale);
		 loco.plotLine(topHingeLine, ColorRGBA.Blue);

		 Vector3f[] bottomHingeLine = new Vector3f[2];
		 bottomHingeLine[0] = new Vector3f(3.77f * loco.scale, 0.69f * loco.scale, 12f * loco.scale); //out, up, forward
		 bottomHingeLine[1] = new Vector3f(3.77f * loco.scale, 0.69f * loco.scale, -12f * loco.scale);
		 loco.plotLine(bottomHingeLine, ColorRGBA.Blue);

		 Vector3f[] middleArmLine = new Vector3f[2];
		 middleArmLine[0] = new Vector3f(3.55f * loco.scale, 1.4f * loco.scale, 0.35f * loco.scale); //out, up, forward
		 middleArmLine[1] = new Vector3f(3.77f * loco.scale, 0.69f * loco.scale, 0.35f * loco.scale);
		 loco.plotLine(middleArmLine, ColorRGBA.Blue);
		 */

		ModelParser visitor = new ModelParser(this, assetManager);
		model.breadthFirstTraversal(visitor);

		model.scale(scale);
		model.setCullHint(Spatial.CullHint.Never);
		model.setShadowMode(RenderQueue.ShadowMode.CastAndReceive);

		attachChild(model);
	}
}
